#一阶多项式卡尔曼滤波与正弦测量

# 无高度和速度的先验信息
import matplotlib.pyplot as plt
import numpy as np
import math

delta = 0.1  #采样间隔
T = 30  #采样总时间
K = list(range(1, int(T / delta) + 1))  #采样次数
#实物信息

t_0 = np.arange(0, T, 0.01)  # 未采样前连续时间
Xk = [0] * len(t_0)
XkD = [0] * len(t_0)

#真实信号，第一状态
for i in range(0, len(t_0)):
    Xk[i] = math.sin(t_0[i])
#一阶导，第二状态
for i in range(0, len(t_0)):
    XkD[i] = math.cos(t_0[i])

#基本操作
Pk = np.array([[99999999,0],[0,99999999]])
φk = np.array([[1,delta],[0,1]])  #基本矩阵
H = np.array([[1,0]])  #测量矩阵
Rk = 1.0 #都为标量

#添加过程噪声
PHIS = 0
Q = np.array([[PHIS*(delta**5)/20,PHIS*(delta**4)/8,PHIS*(delta**3)/6],
              [PHIS*(delta**4)/8,PHIS*(delta**3)/3,PHIS*delta*delta/2],
              [PHIS*(delta**3)/6,PHIS*delta*delta/2,PHIS*delta]]);
xhat_prev = np.array([[0],[0]])
xhat = np.array([[0],[0]])

x_measure = [0] * len(K)
t = [0] * len(K)
vk = [0] * len(K)

#计算测量值
for i in range(0, len(K)):
    t[i] = (K[i] - 1) * delta
    vk[i] = np.random.normal(loc = 0, scale=1.0, size= None)
    #x_measure[i] = A0 + A1 * t[i] + A2 * t[i]**2 + vk[i]
    #注意此时的测量值不再是这么简单的相加了
    #Z = H * X + VK;
    x_true = np.array([math.sin(t[i])],[math.cos(t[i])])
    x_measure[i] = (H.dot(x_true)) + vk[i]

for i in range(0,len(K)):
    φk_T = np.transpose(φk)
    H_T = np.transpose(H)
    Mk = φk.dot(Pk).dot(φk_T) + Q                    #3*3
    CC = np.linalg.inv(H.dot(Mk).dot(H_T)+Rk)
    Kk = Mk.dot(H_T).dot(CC)                     #3*1
    Pk = (np.eye(2) - Kk.dot(H)).dot(Mk)         #3*3
    x = φk.dot(xhat_prev) + Kk.dot(x_measure[i]-H.dot(φk).dot(xhat_prev))
    xhat_prev = x
    xhat = np.c_[xhat, x]  # np.c_是按行连接两个矩阵，就是把两矩阵左右相加，要求行数相等。
xhat = np.delete(xhat, 0, axis=1) #删除第0列

#第一状态与真实值
plt.figure()
plt.xlabel("Time(Sec)")
plt.ylabel("xhat")
plt.plot(t_0,Xk,c="b",label = 'true')
plt.plot(t,xhat[0], c="k",label = 'estimates')
plt.legend()
plt.show()

#第二状态与真实值
plt.figure()
plt.xlabel("Time(Sec)")
plt.ylabel("xhatD")
plt.plot(t_0,XkD,'--',c = 'r',label = 'true')
plt.plot(t,xhat[1], c="k",label = 'estimates')
plt.legend()
plt.show()




